#include <zephyr/kernel.h>
#include <zephyr/shell/shell.h>
#include <version.h>
#include <zephyr/logging/log.h>
#include <stdlib.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/usb/usb_device.h>
#include <ctype.h>
#include <zephyr/arch/cpu.h>
#include <zephyr/sys/util.h>
#include <zephyr/linker/linker-defs.h>
#include <zephyr/device.h>
#include <zephyr/shell/shell_uart.h>
#include <zephyr/drivers/flash.h>
#include <zephyr/irq.h>
#include <zephyr/drivers/watchdog.h>
#include <zephyr/devicetree.h>
#include "include/motor.h"
#include "include/relay.h"
LOG_MODULE_REGISTER(Motor);

extern const struct device *uart_motor;
Motor_Para Motor_Para_g;
Motor_Ctr 	Motor_Ctr_g;
K_MUTEX_DEFINE(motor_ctr_mtx);

int Motor_Ctr_API(const struct device *uart_dev, Motor_Ctr rs485_frame){
	if (!device_is_ready(uart_motor)) {
        /* Not ready, do not use */
		LOG_ERR("motor uart device is not ready!");
        return -ENODEV;
	}
	else{
		k_mutex_lock(&motor_ctr_mtx,K_FOREVER);

		uint8_t *data = (uint8_t *)&rs485_frame;
    	uint16_t crc16 = CRC16_ModBus(data,RS485_FRAME_LEN);
   	 	rs485_frame.crc16L = (uint8_t)crc16;
    	rs485_frame.crc16H = (uint8_t)(crc16 >> 8);
    	for(int i = 0; i < RS485_FRAME_LEN_CRC; ++i){//发送帧
	 		uart_poll_out(uart_dev,*(data + i));
		}
		k_sleep(K_MSEC(10));
		Motor_Ctr_g = rs485_frame;
    	k_mutex_unlock(&motor_ctr_mtx);
		return 0;
	}
}

Motor_Ctr Read_PID(void){ //读取pid参数
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_PID;

	return rs485_frame;
}

Motor_Ctr Write_PID(uint8_t Ckp,uint8_t Cki,uint8_t Skp,uint8_t Ski,uint8_t Pkp,uint8_t Pki){ //读取pid参数
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = WRITE_PID;
    rs485_frame.null = 0x00;
    rs485_frame.data[0] = Ckp;
    rs485_frame.data[1] = Cki;
    rs485_frame.data[2] = Skp;
    rs485_frame.data[3] = Ski;
    rs485_frame.data[4] = Pkp;
    rs485_frame.data[5] = Pki;
	return rs485_frame;
}

Motor_Ctr Write_PID_toROM(uint8_t Ckp,uint8_t Cki,uint8_t Skp,uint8_t Ski,uint8_t Pkp,uint8_t Pki){ //读取pid参数
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = WRITE_PID_TOROM;
    rs485_frame.null = 0x00;
    rs485_frame.data[0] = Ckp;
    rs485_frame.data[1] = Cki;
    rs485_frame.data[2] = Skp;
    rs485_frame.data[3] = Ski;
    rs485_frame.data[4] = Pkp;
    rs485_frame.data[5] = Pki;
	return rs485_frame;
}

Motor_Ctr Read_Acc(void){ //读取加速度值
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_ACC;

	return rs485_frame;
}

Motor_Ctr Write_Acc(uint32_t acc){ //acc 单位为 dps/s 范围是50-80000
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = WRITE_ACC;
	rs485_frame.null = 0x00;
	rs485_frame.data[0] = 0x00;
	rs485_frame.data[1] = 0x00;
	rs485_frame.data[2] = (uint8_t)acc; 
	rs485_frame.data[3] = (uint8_t)(acc >> 8); 
	rs485_frame.data[4] = (uint8_t)(acc >> 16);
	rs485_frame.data[5] = (uint8_t)(acc >> 24); 

	return rs485_frame;
}

Motor_Ctr Read_Encoder_pos(void){ //读取绝对编码器位置角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_ENCODER_POS;

	return rs485_frame;
}

Motor_Ctr Read_Encoder_pos_original(void){ //读取相对编码器位置角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_ENCODER_POS_ORIGINAL;

	return rs485_frame;
}

Motor_Ctr Read_Encoder_init_pos(void){ //读取初始编码器位置角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_ENCODER_INIT_POS;

	return rs485_frame;
}

Motor_Ctr Write_Encoder_init_pos(int32_t pos){ //设置初始编码器位置角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = WRITE_ENCODER_INIT_POS;
    rs485_frame.data[2] = (uint8_t)pos; 
	rs485_frame.data[3] = (uint8_t)(pos >> 8); 
	rs485_frame.data[4] = (uint8_t)(pos >> 16); 
	rs485_frame.data[5] = (uint8_t)(pos >> 24); 
	return rs485_frame;
}

Motor_Ctr Write_Encoder_init_pos_toROM(int32_t pos){ //设置初始编码器位置角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = WRITE_ENCODER_INIT_POS_TOROM;

	return rs485_frame;
}

Motor_Ctr Read_abs_angle(void){ //读取当前绝对角度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_ABS_ANGLE;

	return rs485_frame;
}

Motor_Ctr Read_Status_one(void){ //读取电机温度、电压和错误状态
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_STATUS_ONE;

	return rs485_frame;
}

Motor_Ctr Read_Status_two(void){ //读取电机温度、速度和编码器位置
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = READ_STATUS_TWO;

	return rs485_frame;
}

Motor_Ctr Motor_Shutdown(void){ //关闭电机
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = MOTOR_SHUTDOWM;

	return rs485_frame;
}

Motor_Ctr Motor_Stop(void){ //电机停止转动
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = MOTOR_STOP;

	return rs485_frame;
}

Motor_Ctr Motor_Reset_system(void){ //电机系统重置
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = MOTOR_RESET_SYSTEM;

	return rs485_frame;
}

Motor_Ctr Torque_ClosedLoop_ctr(uint32_t toque){ //torque 单位为 A
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = TORQUE_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	uint16_t toqueControl = toque * 100;
	rs485_frame.data[0] = 0x00;
	rs485_frame.data[1] = 0x00;
	rs485_frame.data[2] = (uint8_t)toqueControl; 
	rs485_frame.data[3] = (uint8_t)(toqueControl >> 8); 
	rs485_frame.data[4] = 0x00; 
	rs485_frame.data[5] = 0x00; 

	return rs485_frame;
}

Motor_Ctr Speed_ClosedLoop_ctr(uint32_t speed){ //speed 单位为 dps
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = SPEED_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	uint32_t speedControl = speed * 100;
	rs485_frame.data[0] = 0x00;
	rs485_frame.data[1] = 0x00;
	rs485_frame.data[2] = (uint8_t)speedControl; 
	rs485_frame.data[3] = (uint8_t)(speedControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(speedControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(speedControl >> 24); 
	
	return rs485_frame;
}

Motor_Ctr Pos_Track_closedLoop_ctr(int32_t pos){ //pos 单位为 度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = POS_TRACK_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	int32_t posControl = pos * 100;
	rs485_frame.data[0] = 0x00;
	rs485_frame.data[1] = 0x00;
	rs485_frame.data[2] = (uint8_t)posControl; 
	rs485_frame.data[3] = (uint8_t)(posControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(posControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(posControl >> 24); 

	return rs485_frame;
}

Motor_Ctr Abs_Pos_closedLoop_ctr(int32_t abs,uint16_t maxSpeed){ //abs 单位为 度 ,maxSpeed 单位为 dps
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = ABS_POS_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	int32_t absControl = abs * 100;
	rs485_frame.data[0] = (uint8_t)maxSpeed;
	rs485_frame.data[1] = (uint8_t)(maxSpeed >> 8);
	rs485_frame.data[2] = (uint8_t)absControl; 
	rs485_frame.data[3] = (uint8_t)(absControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(absControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(absControl >> 24); 

	return rs485_frame;
}

Motor_Ctr Pos_Track_ctr_speedLimit(int32_t pos,uint16_t maxSpeed){ //pos 单位为 度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = POS_TRACK_CTR_SPEEDLIMIT;
	rs485_frame.null = 0x00;
	int32_t posControl = pos * 100;
	rs485_frame.data[0] = (uint8_t)maxSpeed;
	rs485_frame.data[1] = (uint8_t)(maxSpeed >> 8);
	rs485_frame.data[2] = (uint8_t)posControl; 
	rs485_frame.data[3] = (uint8_t)(posControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(posControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(posControl >> 24); 

	return rs485_frame;
}

Motor_Ctr Pos_Inc_closedLoop_ctr(int32_t pos,uint16_t maxSpeed){ //pos 单位为 度
	Motor_Ctr rs485_frame = {0};
	rs485_frame.frame_header = 0x3E;
	rs485_frame.id = 0x01;
	rs485_frame.length = 0x08;
	rs485_frame.cmd = POS_INC_CLOSEDLOOP_CTR;
	rs485_frame.null = 0x00;
	int32_t posControl = pos * 100;
	rs485_frame.data[0] = (uint8_t)maxSpeed;
	rs485_frame.data[1] = (uint8_t)(maxSpeed >> 8);
	rs485_frame.data[2] = (uint8_t)posControl; 
	rs485_frame.data[3] = (uint8_t)(posControl >> 8); 
	rs485_frame.data[4] = (uint8_t)(posControl >> 16); 
	rs485_frame.data[5] = (uint8_t)(posControl >> 24); 

	return rs485_frame;
}

#define MOTOR_485_MSGQ_MAX 10
/* queue to store up to 10 messages (aligned to 4-byte boundary) */
K_MSGQ_DEFINE(uart_motor_rx_msgq, RS485_FRAME_LEN_CRC, MOTOR_485_MSGQ_MAX, 4);

/* receive buffer used in UART ISR callback */
static uint8_t rx_buf[RS485_FRAME_LEN_CRC];
static int rx_buf_pos;
static bool rx_flag;


void uart_motor_rx_irq_cb(const struct device *uart_dev,void *user_data)
{
	uint8_t c;
	if (!uart_irq_update(uart_dev)) {
		return;
	}
	while (uart_irq_rx_ready(uart_dev)) {

		uart_fifo_read(uart_dev, &c, 1);
        if(c == RS485_FRAME_HEAD){
            if(rx_buf_pos != 0){
                rx_buf_pos = 0;
            }
            rx_flag = true;
        }
            
        if(rx_flag){
            rx_buf[rx_buf_pos++] = c;
        }
        if(rx_buf_pos == RS485_FRAME_LEN_CRC){
            k_msgq_put(&uart_motor_rx_msgq, &rx_buf, K_NO_WAIT);
            rx_buf_pos = 0;
            rx_flag = false;
        }
	}

}


void rs485_rx_dispense(void *p1, void *p2, void *p3)
{
    while(1){
        Motor_Ctr rs485_frame = {0};
        if(k_msgq_get(&uart_motor_rx_msgq,&rs485_frame, K_FOREVER) == 0){
            uint16_t crc16 = CRC16_ModBus((uint8_t *)&rs485_frame,RS485_FRAME_LEN);
            if(rs485_frame.crc16L != (uint8_t)crc16 ||  rs485_frame.crc16H != (uint8_t)(crc16 >> 8)){
                LOG_ERR("crc check error");
                continue;
            }
            switch(rs485_frame.cmd){
                case READ_PID://读取pid参数 
                case WRITE_PID:{        
                    Motor_Para_g.CurrKP = rs485_frame.data[0] * Motor_Para_g.motor_curr / 256;
                    Motor_Para_g.CurrKI = rs485_frame.data[1] * Motor_Para_g.motor_curr / 256;
                    Motor_Para_g.SpdKP = rs485_frame.data[2] * Motor_Para_g.motor_spd / 256;
                    Motor_Para_g.SpdKI = rs485_frame.data[3] * Motor_Para_g.motor_spd / 256;
                    Motor_Para_g.PosKP = rs485_frame.data[4] * Motor_Para_g.motor_degree / 256;
                    Motor_Para_g.PosKP = rs485_frame.data[5] * Motor_Para_g.motor_degree / 256;
                    break; 
                }
                case READ_ACC://读取加速度值 单位dps/s
                case WRITE_ACC:{
                    Motor_Para_g.motor_acc = rs485_frame.data[2] | (rs485_frame.data[3] << 8) |  (rs485_frame.data[4] << 16) |  (rs485_frame.data[5] << 24);
                    break;
                }
                case READ_ENCODER_POS:{ //读取绝对编码器位置 单位脉冲
                    Motor_Para_g.encoder_pos = rs485_frame.data[2] | (rs485_frame.data[3] << 8) |  (rs485_frame.data[4] << 16) |  (rs485_frame.data[5] << 24);
                    break;
                }
                case READ_ENCODER_POS_ORIGINAL:{//读取相对编码器位置 单位脉冲
                    Motor_Para_g.encoder_pos_original = rs485_frame.data[2] | (rs485_frame.data[3] << 8) |  (rs485_frame.data[4] << 16) |  (rs485_frame.data[5] << 24);
                    break;
                }
                case READ_ENCODER_INIT_POS:
                case WRITE_ENCODER_INIT_POS:{//读取编码器原始位置 单位脉冲
                    Motor_Para_g.encoder_pos_init = rs485_frame.data[2] | (rs485_frame.data[3] << 8) |  (rs485_frame.data[4] << 16) |  (rs485_frame.data[5] << 24);
                    break;
                }
                case READ_ABS_ANGLE:{ //读取绝对角度 单位度
                    Motor_Para_g.abs_angle = (float)((rs485_frame.data[2] | (rs485_frame.data[3] << 8) |  (rs485_frame.data[4] << 16) |  (rs485_frame.data[5] << 24)) / 100);
                    break;
                }
                case READ_STATUS_ONE:{ //读取电机状态1 
                    Motor_Para_g.motor_tem = rs485_frame.null; 
                    Motor_Para_g.motor_vol = (float)((rs485_frame.data[2] | (rs485_frame.data[3] << 8)) / 10);
                    break;
                }
                case READ_STATUS_TWO:{ //读取电机状态2 
                    Motor_Para_g.motor_tem = rs485_frame.null; 
                    Motor_Para_g.motor_curr = (float)(((rs485_frame.data[0] | (rs485_frame.data[1] << 8))) < (65535 / 2))?((rs485_frame.data[0] | (rs485_frame.data[1] << 8)) * 0.01):0;
                    Motor_Para_g.motor_spd = (float)(rs485_frame.data[2] | (rs485_frame.data[3] << 8));
                    Motor_Para_g.motor_degree = (float)(rs485_frame.data[4] | (rs485_frame.data[5] << 8));
                    break;
                }
                case TORQUE_CLOSEDLOOP_CTR:
                case SPEED_CLOSEDLOOP_CTR:
                case POS_TRACK_CLOSEDLOOP_CTR:
                case ABS_POS_CLOSEDLOOP_CTR:
                case POS_TRACK_CTR_SPEEDLIMIT:
                case POS_INC_CLOSEDLOOP_CTR:{ //读取电机状态
                    Motor_Para_g.motor_tem = rs485_frame.null; 
                    Motor_Para_g.motor_curr = (float)((rs485_frame.data[0] | (rs485_frame.data[1] << 8)) * 0.01);
                    Motor_Para_g.motor_spd = (float)(rs485_frame.data[2] | (rs485_frame.data[3] << 8));
                    Motor_Para_g.motor_degree = (float)(rs485_frame.data[4] | (rs485_frame.data[5] << 8));
                    break;
                }
                default:{
                    LOG_ERR("unknown response");
                    break;
                }
            }
        }
		k_sleep(K_MSEC(10));
    }
}

int motor_init(const struct device *uart_dev)
{
	if (!device_is_ready(uart_motor)) {
        /* Not ready, do not use */
		LOG_ERR("motor uart device is not ready!");
        return -ENODEV;
	}
	else{
		uart_irq_callback_user_data_t uart_motor_rx_irq_t = uart_motor_rx_irq_cb;
		uart_irq_rx_disable(uart_dev);
    	uart_irq_callback_set(uart_dev,uart_motor_rx_irq_t);
		uart_irq_rx_enable(uart_dev);
		return 0;
	}
}